import numpy as np
import matplotlib.pyplot as plt


plt.rcParams['font.sans-serif'] = ['Microsoft YaHei']
plt.rcParams['axes.unicode_minus'] = False
plt.switch_backend('TkAgg')

# -------------------------- 1. 基础参数定义 --------------------------
target_true = np.array([0, 200, 0])  # 真目标底面圆心
r, h = 7, 10  # 真目标的半径7m、高10m
target_fake = np.array([0, 0, 0])  # 假目标

missile_M1 = np.array([20000, 0, 2000])  # M1初始位置
missile_speed = 300  # 导弹速度300m/s
drone_FY1 = np.array([17800, 0, 1800])  # FY1初始位置
drone_speed = 120  # FY1速度120m/s

g = 9.8  # 重力加速度(m/s²)
smoke_radius = 10  # 烟幕有效半径10m
smoke_sink_speed = 3  # 烟幕下沉速度3m/s
smoke_valid_time = 20  # 烟幕起爆后20s有效
drone_drop_delay = 1.5  # 受领任务后1.5s投放
smoke_detonate_delay = 3.6  # 投放后3.6s起爆
detonate_t = drone_drop_delay + smoke_detonate_delay  # 烟幕起爆时刻（固定5.1s）


# -------------------------- 2. 真目标外接长方体顶点生成 --------------------------
def get_cylinder_bounding_box_vertices(target_center, radius, height):
    x_min = target_center[0] - radius
    x_max = target_center[0] + radius
    y_min = target_center[1] - radius
    y_max = target_center[1] + radius
    z_min = target_center[2]
    z_max = target_center[2] + height
    #长方体顶点
    vertices = [
        np.array([x_min, y_min, z_min]),
        np.array([x_max, y_min, z_min]),
        np.array([x_max, y_max, z_min]),
        np.array([x_min, y_max, z_min]),
        np.array([x_min, y_min, z_max]),
        np.array([x_max, y_min, z_max]),
        np.array([x_max, y_max, z_max]),
        np.array([x_min, y_max, z_max])
    ]
    return vertices


# -------------------------- 3. 实体运动计算 --------------------------
def missile_real_time_position(missile_init_pos, t):
    if t < 0:
        return missile_init_pos
    # 导弹方向向量，从初始位置指向假目标
    dir_vec = target_fake - missile_init_pos
    dir_vec_mag = np.linalg.norm(dir_vec)
    dir_unit_vec = dir_vec / dir_vec_mag if dir_vec_mag > 1e-6 else np.array([0, 0, 0])
    # 实时位置
    displacement = missile_speed * t * dir_unit_vec
    return missile_init_pos + displacement


def get_smoke_explosion_center():
    # 1. 投放点坐标
    drop_x = drone_FY1[0] - drone_speed * drone_drop_delay
    drop_y = drone_FY1[1]
    drop_z = drone_FY1[2]

    # 2. 烟幕弹平抛运动
    explosion_x = drop_x - drone_speed * smoke_detonate_delay
    explosion_y = drop_y
    # 竖直自由下落
    fall_distance = 0.5 * g * (smoke_detonate_delay ** 2)
    explosion_z = max(drop_z - fall_distance, 0.0)

    return np.array([round(explosion_x, 1), round(explosion_y, 1), round(explosion_z, 1)])


def smoke_real_time_center(t):
    if t < detonate_t - 1e-6:
        return np.array([0, 0, 0])
    # 起爆时刻中心
    explosion_center = get_smoke_explosion_center()
    # 下沉距离
    sink_distance = smoke_sink_speed * (t - detonate_t)
    real_z = max(explosion_center[2] - sink_distance, 0.0)
    return np.array([explosion_center[0], explosion_center[1], real_z])


# -------------------------- 4. 遮蔽判断 --------------------------
def is_segment_sphere_intersect(missile_pos, vertex_pos, smoke_center, smoke_radius):
    vec_AC = smoke_center - missile_pos
    vec_AB = vertex_pos - missile_pos
    ab_sq_mag = np.dot(vec_AB, vec_AB)
    if ab_sq_mag < 1e-12:
        return False
    # 参数k：定位线段上离云团最近的点
    k = np.dot(vec_AC, vec_AB) / ab_sq_mag
    # 计算最短距离
    if k <= 0.0:
        min_dist = np.linalg.norm(vec_AC)
    elif 0.0 < k < 1.0:
        # 最近点在线段内：叉积模长 / 线段长度
        cross_prod = np.cross(vec_AC, vec_AB)
        min_dist = np.linalg.norm(cross_prod) / np.sqrt(ab_sq_mag)
    else:
        vec_BC = smoke_center - vertex_pos
        min_dist = np.linalg.norm(vec_BC)
    return min_dist <= smoke_radius + 1e-6


def is_effective_occlusion(t):
    # 1. 时间有效性
    smoke_start = detonate_t
    smoke_end = detonate_t + smoke_valid_time
    if t < 0 or t < smoke_start - 1e-6 or t > smoke_end + 1e-6:
        return False
    # 2. 计算实时位置
    missile_pos = missile_real_time_position(missile_M1, t)
    target_vertices = get_cylinder_bounding_box_vertices(target_true, r, h)
    smoke_center = smoke_real_time_center(t)

    # 3. 只要1个顶点视线被挡，即有效遮蔽
    for vertex in target_vertices:
        if is_segment_sphere_intersect(missile_pos, vertex, smoke_center, smoke_radius):
            return True
    return False


# -------------------------- 5. 有效遮蔽时长计算 --------------------------
def calculate_effective_duration(time_step=0.001):
    smoke_start = detonate_t
    smoke_end = detonate_t + smoke_valid_time
    total_duration = 0.0
    # 判断连续区间
    prev_effective = False

    # 遍历时间区间
    t = smoke_start
    while t <= smoke_end + 1e-6:
        current_effective = is_effective_occlusion(t)
        if current_effective and not prev_effective:
            start_t = t
        elif not current_effective and prev_effective:
            total_duration += t - start_t
        prev_effective = current_effective
        t += time_step
    if prev_effective:
        total_duration += smoke_end - start_t
    return round(total_duration, 3)


# -------------------------- 6. 执行与结果输出 --------------------------
if __name__ == "__main__":
    effective_duration = calculate_effective_duration()
    print(f"\n=== 问题1结果 ===")
    print(f"烟幕对M1的有效遮蔽时长：{effective_duration} 秒")